#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include "std_msgs/msg/float64.hpp"
#include <sensor_msgs/point_cloud2_iterator.hpp>
#include <std_msgs/msg/float32_multi_array.hpp>

class Ros2Ros : public rclcpp::Node
{
public:
    Ros2Ros() : Node("ros2ros")
    {
        sub_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant);
        rclcpp::SubscriptionOptions options_sub;
        options_sub.callback_group = sub_callback_group_;

        pub_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant);

        // 订阅点云话题
        subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
            "/line_point", 10,
            [this](const sensor_msgs::msg::PointCloud2::SharedPtr msg)
            {
                this->pointCloudCallback(msg);
            },
            options_sub);
        // 订阅压力信息
        pressure_subscription_joint2 = this->create_subscription<std_msgs::msg::Float64>(
            "/joint2_pressure", 10,
            [this](const std_msgs::msg::Float64::SharedPtr msg)
            {
                this->joint2pressureCallback(msg);
            },
            options_sub);

        pressure_subscription_joint3 = this->create_subscription<std_msgs::msg::Float64>(
            "/joint3_pressure", 10,
            [this](const std_msgs::msg::Float64::SharedPtr msg)
            {
                this->joint3pressureCallback(msg);
            },
            options_sub);
        // 向constyle4发布感知的信息（倒U）
        to_do_ = this->create_publisher<std_msgs::msg::Float32MultiArray>("float_array_topic", 5);
        // 发布压力信息
        joint2_pressure_pub = this->create_publisher<std_msgs::msg::Float64>("joint2_pressure", 10);
        joint3_pressure_pub = this->create_publisher<std_msgs::msg::Float64>("joint3_pressure", 10);
    }

private:
    rclcpp::CallbackGroup::SharedPtr sub_callback_group_;
    rclcpp::CallbackGroup::SharedPtr pub_callback_group_;
    // 创建回调组
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
    // 创建压力信息订阅
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr pressure_subscription_joint2;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr pressure_subscription_joint3;
    // 创建压力信息发布
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr joint2_pressure_pub;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr joint3_pressure_pub;
    // 创建感知信息发布
    rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr to_do_;

    void joint2pressureCallback(const std_msgs::msg::Float64::SharedPtr msg)
    {
        RCLCPP_INFO(this->get_logger(), "Joint2 Pressure: %f", msg->data);
        auto joint2_pressure = std_msgs::msg::Float64();
        joint2_pressure_pub->publish(joint2_pressure);
    }
    void joint3pressureCallback(const std_msgs::msg::Float64::SharedPtr msg)
    {
        RCLCPP_INFO(this->get_logger(), "Joint3 Pressure: %f", msg->data);
        auto joint3_pressure = std_msgs::msg::Float64();
        joint3_pressure_pub->publish(joint3_pressure);
    }

    void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
    {
        float x_right = 0.0f, x_left = 0.0f;
        float z_high = 0; // 初始化为极小值
        int right_count = 0, left_count = 0;

        sensor_msgs::PointCloud2Iterator<float> iter_x(*msg, "x");
        sensor_msgs::PointCloud2Iterator<float> iter_y(*msg, "y");
        sensor_msgs::PointCloud2Iterator<float> iter_z(*msg, "z");
        sensor_msgs::PointCloud2Iterator<float> iter_intensity(*msg, "intensity"); // 改为float类型

        for (size_t i = 0; i < msg->width * msg->height; ++i)
        {
            float y = *iter_y;
            if (y < 0.6 || y > 29.0)
            {
                // 跳过不符合条件的点
                ++iter_x;
                ++iter_y;
                ++iter_z;
                ++iter_intensity;
                continue;
            }

            float intensity = *iter_intensity; // 读取float类型的intensity

            if (intensity == 1500.0f)
            { // 右边界
                x_right += *iter_x;
                right_count++;
            }
            else if (intensity == 250.0f)
            { // 左边界
                x_left += *iter_x;
                left_count++;
            }

            if (*iter_z > z_high)
            { // 更新最大高度
                z_high = *iter_z;
            }

            ++iter_x;
            ++iter_y;
            ++iter_z;
            ++iter_intensity;
        }

        // 计算左右边界的平均x坐标
        if (right_count > 0)
        {
            x_right /= right_count;
        }
        if (left_count > 0)
        {
            x_left /= left_count;
        }

        // 创建并发布消息
        auto merged_pose = std_msgs::msg::Float32MultiArray();
        merged_pose.data.resize(3);
        merged_pose.data[0] = x_right * 1000; // 右边界x坐标
        merged_pose.data[1] = x_left * 1000;  // 左边界x坐标
        merged_pose.data[2] = z_high * 1000;  // 最大高度

        to_do_->publish(merged_pose);

        // 打印调试信息
        RCLCPP_INFO(this->get_logger(), "右边界值 X: %f, 左边界值 X: %f, 高度(最低): %f",
                    merged_pose.data[0], merged_pose.data[1], merged_pose.data[2]);
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<Ros2Ros>();
    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node);
    executor.spin();
    rclcpp::shutdown();
    return 0;
}